﻿using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using System;
using System.Drawing;

namespace FinalYear.iNavigate.Classes
{
    public class SFM
    {
        public PointF[] CurrentFeatures { get; set; }

        public PointF[] PreviousFeatures { get; set; }

        private int pointCount;

        public SFM(PointF[] currentFeatures, PointF[] previousFeatures)
        {
            this.CurrentFeatures = currentFeatures;
            this.PreviousFeatures = previousFeatures;
            this.pointCount = currentFeatures.Length;
        }

        public void CalculateFundamentalMatrix()
        {
            IntPtr points1 = CreatePointListPointer(this.CurrentFeatures);
            IntPtr points2 = CreatePointListPointer(this.PreviousFeatures);
            IntPtr status = CvInvoke.cvCreateMat(1, this.pointCount, MAT_DEPTH.CV_8U);

            // fundamentalMatrix = CvInvoke.cvCreateMat(3, 3, MAT_DEPTH.CV_32F);

            var fundamentalMatrix = new Matrix<double>(3, 3, 1);
            var epipolarLines = new Matrix<double>(3, 1);
            int fmCount = CvInvoke.cvFindFundamentalMat(points1,
                                                        points2,
                                                        fundamentalMatrix,
                                                        CV_FM.CV_FM_8POINT,
                                                        3.0,
                                                        0.99,
                                                        status);

            //CvInvoke.cvComputeCorrespondEpilines(points1, 1, fundamentalMatrix, epipolarLines);
            //CvInvoke.draw
        }

        public IntPtr CreatePointListPointer(PointF[] points)
        {
            IntPtr result = CvInvoke.cvCreateMat(this.pointCount, 2, MAT_DEPTH.CV_32F);

            for (int i = 0; i < pointCount; i++)
            {
                double currentX = points[i].X;
                double currentY = points[i].Y;
                CvInvoke.cvSet2D(result, i, 0, new MCvScalar(currentX));
                CvInvoke.cvSet2D(result, i, 1, new MCvScalar(currentY));
            }

            return result;
        }

        public PointF GetIntersection(LineSegment2D line1, LineSegment2D line2)
        {
            double a1 = (line1.P1.Y - line1.P2.Y) / (double)(line1.P1.X - line1.P2.X);
            double b1 = line1.P1.Y - a1 * line1.P1.X;

            double a2 = (line2.P1.Y - line2.P2.Y) / (double)(line2.P1.X - line2.P2.X);
            double b2 = line2.P1.Y - a2 * line2.P1.X;

            if (Math.Abs(a1 - a2) < double.Epsilon)
                throw new InvalidOperationException();

            double x = (b2 - b1) / (a1 - a2);
            double y = a1 * x + b1;
            return new PointF((float)x, (float)y);
        }
    }
}